/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Watchdog;
import java.util.Date;
import java.util.Timer;
import java.util.TimerTask;

public class DefaultRobot extends IterativeRobot
{
    DriverStation m_ds;
    Joystick driverJoy;
    Joystick shooterJoy;
    Kicker kicker;

    long millis;

    Timer dtTimer;
    Timer mmTimer;
    boolean mmTimerStarted = false;
    boolean mmEntered = false;
    private Compressor compressor;

    boolean shutdownInit = false;
    boolean shutdownFinished = false;

    boolean dtInitialized = false;

    TimerTask maintModeTT;

    boolean cleanupMM = false;

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit()
    {
        m_ds = DriverStation.getInstance();

        compressor = new Compressor(6,5);
        compressor.start();

        kicker = new Kicker(shooterJoy);

    }



    /********************************** Init Routines *************************************/
    public void disabledInit()
    {
    }

    public void autonomousInit()
    {

    }

    public void teleopInit()
    {

    }

    /********************************** Periodic Routines *************************************/
    public void disabledPeriodic()
    {
        // feed the user watchdog at every period when disabled
        Watchdog.getInstance().feed();

    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic()
    {
        Watchdog.getInstance().feed();
    }

    /**
     * This function is called periodically during operator control. Check whether
     * the kicker is pressing a button on the joystick. All DT tasks are being
     * completed in a separate thread.
     */
    public void teleopPeriodic()
    {
        Watchdog.getInstance().feed();

        kicker.Run();
        lifter.Run();




//        if(driverJoy.getRawButton(Utils.TARGET_TUNE_BUTTON_NUM))
//        {
//            dt.getCurrPID().disable();
//            TargetTuner.tuneTarget(driverJoy);
//            dt.getCurrPID().setSetpoint(TargetTuner.TARGET);
//            dt.getCurrPID().enable();
//        }

//        if(driverJoy.getRawButton(Utils.PID_TUNE_BUTTON_NUM))
//        {
//            dt.getCurrPID().disable();
//            PIDDebugUtil.enterPIDDebug(dt.getCurrPID(), driverJoy, "None");
//            dt.getCurrPID().enable();
//
//        }
//        System.out.println("------------------------");
//        System.out.println("Raw value: " + dt.getCurrEnc().getValue());
//        System.out.println("Numturns: " + dt.getCurrEnc().numTurns);
//        System.out.println("Calc angle: " + dt.getCurrEnc().getCurrentAngle());
//        System.out.println("Error: " + dt.getCurrPID().getError());
//        System.out.println("Output: " + dt.getCurrPID().get());
    }
}
